#include "mpu9250.h"


//struct mpu9250_data mpu9250_data;
//ASA_X: 174   ASA_Y: 173   ASA_Z: 163  ID:    0x48
//unsigned char AK_ID = 0;
//unsigned char ASA_X = 174;
//unsigned char ASA_Y = 173;
//unsigned char ASA_Z = 163;

float mpu9250_gyro_x_offset = -0.0143f;
float mpu9250_gyro_y_offset = 0.0038f;
float mpu9250_gyro_z_offset = -0.0232f;


//float Mag_Bias_X = 0.0f;
//float Mag_Bias_Y = 0.0f;
//float Mag_Bias_Z = -0.0f;
//float Mag_Scale[6] = {0.970014462720998f,
//                      -0.00699774522446753f,
//                      -0.0174402698381483f,
//                      0.988601807599842f,
//                      0.056586439235578f,
//                      1.04639002054441f
//                      };


float mpu9250_accel_x_scale = 0.9960f;
float mpu9250_accel_y_scale = 0.9981f;
float mpu9250_accel_z_scale = 0.9838f;
float mpu9250_accel_x_offset = -0.0025f;
float mpu9250_accel_y_offset = -0.0001f;
float mpu9250_accel_z_offset = 0.1803f;

#ifdef USING_RTT_OS
void rt_hw_us_delay(rt_uint32_t us)
{
    rt_uint32_t delta;
    /* 获得延时经过的 tick 数 */
    us = us * (SysTick->LOAD / (1000000 / RT_TICK_PER_SECOND));
    /* 获得当前时间 */
    delta = SysTick->VAL;
    /* 循环获得当前时间，直到达到指定的时间后退出循环 */
    while (delta - SysTick->VAL < us);
}
#endif

void mpu9250_mdelay(signed int ms)
{
#ifdef USING_RTT_OS
    rt_thread_mdelay(ms);//note函数会引起线程挂起
#else
    delay_ms(ms);//其他非RTOS类延时
#endif
}

void mpu9250_udelay(signed int us)
{
#ifdef USING_RTT_OS
    unsigned short rtt_us_delay_max;//rt-thread OS maximum us dalay count
    rtt_us_delay_max = 1 / (RT_TICK_PER_SECOND) * 1000 * 1000;
    if (us >= rtt_us_delay_max)
    {
        us = rtt_us_delay_max - 1;
    }
    rt_hw_us_delay(us);
#else
    delay_us(us);//其他非RTOS类延时
#endif
}

/** How To Use
*   ak8963_init_iic(&ASA_X, &ASA_Y, &ASA_Z, &AK_ID);
*   mpu9250_mdelay(100);
*   Init_MPU9250_on_IIC();
*/

unsigned char mpu9250_read_reg(unsigned char reg_addr)
{
    unsigned char RxData = 0;
#ifdef USE_IIC_PROTOCAL
    RxData = Single_Read(MPU9250_IIC_ADDR, reg_addr);
#else
    CS1_L;
    CS1_L;
    SPI2_ReadWriteByte(reg_addr | 0x80);
    RxData = SPI2_ReadWriteByte(0xff);
    CS1_H;
    mpu9250_udelay(50);
#endif
    return RxData;
}

void mpu9250_write_reg(unsigned char reg_addr, unsigned char data)
{

#ifdef USE_IIC_PROTOCAL
    Single_Write(MPU9250_IIC_ADDR, reg_addr, data);
#else
    CS1_L;
    CS1_L;
    SPI2_ReadWriteByte(Addr);
    SPI2_ReadWriteByte(TxData);
    CS1_H;
    mpu9250_udelay(100);
#endif
}

void ak8963_write_reg(unsigned char reg_addr, unsigned char data)
{
    mpu9250_write_reg(I2C_SLV0_CTRL, 0x00);

    mpu9250_write_reg(I2C_SLV0_ADDR, AK8963_IIC_ADDR >> 1); //设置地址且为写
    mpu9250_write_reg(I2C_SLV0_REG, reg_addr);
    mpu9250_write_reg(I2C_SLV0_DO, data);
    mpu9250_write_reg(I2C_SLV0_CTRL, 0x81);
    mpu9250_mdelay(20);
}

unsigned char ak8963_read_reg(unsigned char reg_addr)
{
    unsigned char RxData;

    mpu9250_write_reg(I2C_SLV0_CTRL, 0x00);
    mpu9250_write_reg(I2C_SLV0_ADDR, 0x80 | (AK8963_IIC_ADDR >> 1)); //设置地址且为读
    mpu9250_write_reg(I2C_SLV0_REG, reg_addr);
    mpu9250_write_reg(I2C_SLV0_CTRL, 0x81);
    mpu9250_mdelay(20);
    RxData = mpu9250_read_reg(EXT_SENS_DATA_00);
    return RxData;
}

//void ak8963_read_magnetometer(void)
//{
//    mpu9250_mdelay(5);
//    mpu9250_write_reg(I2C_SLV0_ADDR, 0x80 | (AK8963_IIC_ADDR >> 1)); //设置地址且为读
//    mpu9250_write_reg(I2C_SLV0_REG, ST1);

//    mpu9250_write_reg(I2C_SLV0_CTRL, 0x88); //读取长度为8

//    mpu9250_udelay(50);
//}


//static void ak8963_init_iic(unsigned char *ASA_X, unsigned char *ASA_Y, unsigned char *ASA_Z, unsigned char *whoami)
//{
//#ifdef USE_IIC_PROTOCAL
//    Single_Write(MPU9250_IIC_ADDR, INT_PIN_CFG, 0x02);//by-pass enable

//    Single_Write(AK8963_IIC_ADDR, CNTL2, (0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 1 << 0)); //restart

//    if (Single_Read(AK8963_IIC_ADDR, WIA) != 255)
//    {
//        *whoami = Single_Read(AK8963_IIC_ADDR, WIA);   //read ID
//    }
//    mpu9250_mdelay(100);

//    Single_Write(AK8963_IIC_ADDR, CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 1 << 3 | 1 << 2 | 1 << 1 | 1 << 0)); //Fuse ROM access mode
//    mpu9250_mdelay(100);
//    if (Single_Read(AK8963_IIC_ADDR, ASAX) != 255)
//    {
//        *ASA_X = Single_Read(AK8963_IIC_ADDR, ASAX);   //read Sensitivity adjustment data for each axis
//    }
//    mpu9250_mdelay(10);
//    if (Single_Read(AK8963_IIC_ADDR, ASAY) != 255)
//    {
//        *ASA_Y = Single_Read(AK8963_IIC_ADDR, ASAY);
//    }
//    mpu9250_mdelay(10);
//    if (Single_Read(AK8963_IIC_ADDR, ASAZ) != 255)
//    {
//        *ASA_Z = Single_Read(AK8963_IIC_ADDR, ASAZ);
//    }
//    mpu9250_mdelay(10);
//    Single_Write(AK8963_IIC_ADDR, CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0)); //power down mode (required!!)
//    mpu9250_mdelay(100);
//    Single_Write(AK8963_IIC_ADDR, CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 1 << 2 | 1 << 1 | 0 << 0)); // Continuous measurement mode 2   100Hz refresh rate
//    mpu9250_mdelay(100);
//    Single_Write(MPU9250_IIC_ADDR, INT_PIN_CFG, 0x00);//by-pass disable
//    mpu9250_mdelay(100);
//#endif
//}

void mpu9250_init()
{
#ifdef USE_IIC_PROTOCAL

    //ak8963_init_iic(&ASA_X, &ASA_Y, &ASA_Z, &AK_ID);
    mpu9250_mdelay(100);
    Single_Write(MPU9250_IIC_ADDR, PWR_MGMT_1, 0x80);//restart
    mpu9250_mdelay(100);
    Single_Write(MPU9250_IIC_ADDR, PWR_MGMT_1, 0x00);
    mpu9250_mdelay(10);

    Single_Write(MPU9250_IIC_ADDR, SMPLRT_DIV, 0x00);
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, CONFIG, GYRO_92Hz_Bandwidth_1KHz_FS_TEMP_98Hz_Bandwidth);//GYro 92Hz BW tpreture 98Hz BW
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, GYRO_CONFIG, GYRO_FS_SEL_2000dps);
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, ACCEL_CONFIG, ACCEL_FS_SEL_16g);
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, ACCEL_CONFIG_2, ACCEL_41Hz_Bandwidth_1KHz_FS);
    mpu9250_mdelay(10);

    Single_Write(MPU9250_IIC_ADDR, I2C_MST_CTRL, 0x5D);//MST_IIC_400Khz
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_ADDR, (1 << 7 | AK8963_IIC_ADDR >> 1)); //Set SLAVE0 ADDR for mag read
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_REG, ST1);//
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_CTRL, (1 << 7 | 8)); //SLV0 ENABLE& disswap even numbered register ends the group &LEN=8
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, I2C_SLV4_CTRL, 0x09);//1000/(9+1)Hz
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, I2C_MST_DELAY_CTRL, 0x81);
    mpu9250_mdelay(10);
    Single_Write(MPU9250_IIC_ADDR, USER_CTRL, IIC_MST_ENABLE);//enable master IIC of MPU9250
#else
//  printf("# step 1  \r\n");
//  SPI2_ReadWriteByte(0<<7|PWR_MGMT_1);//写指令+地址
//  SPI2_ReadWriteByte(0x80);//写数据//restart
//  mpu9250_mdelay(1000);

    mpu9250_write_reg(PWR_MGMT_1, 0x80);
    mpu9250_mdelay(100);
    while (mpu9250_write_reg(USER_CTRL, 0x10)); //禁用IIC丛机，只启用SPI丛机
    mpu9250_mdelay(10);
    while (mpu9250_write_reg(PWR_MGMT_1, 0x00));
    while (mpu9250_write_reg(SMPLRT_DIV, 0x00));
    while (mpu9250_write_reg(CONFIG, GYRO_92Hz_Bandwidth_1KHz_FS_TEMP_98Hz_Bandwidth));
    while (mpu9250_write_reg(GYRO_CONFIG, GYRO_FS_SEL_2000dps));
    while (mpu9250_write_reg(ACCEL_CONFIG, ACCEL_FS_SEL_16g));
    while (mpu9250_write_reg(ACCEL_CONFIG_2, ACCEL_41Hz_Bandwidth_1KHz_FS));

    while (mpu9250_write_reg(INT_PIN_CFG, 0x30));
    while (mpu9250_write_reg(I2C_MST_CTRL, 0x53)); //设置MPU9250 主IIC频率400
    while (mpu9250_write_reg(USER_CTRL, IIC_MST_ENABLE)); //enable master IIC of MPU9250
    //mpu9250_write_reg(I2C_MST_DELAY_CTRL,0x01);
    printf("# step 3  \r\n");
    ak8963_write_reg_spi(CNTL2, (0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 1 << 0)); //重启
    printf("# step 4  \r\n");
    mpu9250_mdelay(500);
    mpu9250_mdelay(500);
    printf("# CNTL is: 0x%X\r\n", ak8963_read_reg_spi(CNTL));
    do
    {
        ak8963_write_reg_spi(CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 1 << 3 | 1 << 2 | 1 << 1 | 1 << 0));
    }
    while (ak8963_read_reg_spi(CNTL) != (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 1 << 3 | 1 << 2 | 1 << 1 | 1 << 0));
    printf("# CNTL is: 0x%X\r\n", ak8963_read_reg_spi(CNTL));

    printf("# step 5  \r\n");

//  if (ak8963_read_reg_spi(ASAX)!=255&&ak8963_read_reg_spi(ASAX)!=0)
//  {
    ASA_X = ak8963_read_reg_spi(ASAX);
//  }
//  if (ak8963_read_reg_spi(ASAY)!=255&&ak8963_read_reg_spi(ASAY)!=0)
//  {
    ASA_Y = ak8963_read_reg_spi(ASAY);
//  }
//  if (ak8963_read_reg_spi(ASAZ)!=255&&ak8963_read_reg_spi(ASAZ)!=0)
//  {
    ASA_Z = ak8963_read_reg_spi(ASAZ);
//  }
    printf("# ASA_X is %d\r\n", ASA_X);
    printf("# ASA_Y is %d\r\n", ASA_Y);
    printf("# ASA_Z is %d\r\n", ASA_Z);

    ak8963_write_reg_spi(CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0));
    printf("# step 6  \r\n");
    printf("# CNTL is: 0x%X\r\n", ak8963_read_reg_spi(CNTL));

    do
    {
        ak8963_write_reg_spi(CNTL, (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 1 << 2 | 1 << 1 | 0 << 0)); //连续测量模式2 100Hz
    }
    while (ak8963_read_reg_spi(CNTL) != (0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 1 << 2 | 1 << 1 | 0 << 0));
    printf("# step 7  \r\n");
    printf("# CNTL is: 0x%X\r\n", ak8963_read_reg_spi(CNTL));



//  while(mpu9250_write_reg(USER_CTRL,IIC_MST_DISABLE));
//  while(mpu9250_write_reg(I2C_MST_DELAY_CTRL,0x00));

//  while(mpu9250_write_reg(I2C_SLV0_ADDR,(1<<7)|(AK8963_IIC_ADDR>>1)));
//  while(mpu9250_write_reg(I2C_SLV0_REG,ST1));
//  while(mpu9250_write_reg(I2C_SLV0_CTRL,(1<<7)|8)){};
//  while(mpu9250_write_reg(I2C_SLV4_CTRL,0x01));

//  while(mpu9250_write_reg(I2C_MST_DELAY_CTRL,0x81));
//  while(mpu9250_write_reg(USER_CTRL,IIC_MST_ENABLE));//enable master IIC of MPU9250
#endif
}

void mpu9250_read_rdata(mpu9250_rdata_t rdata_t)
{
    //读取数据的同时将mpu9250的测量轴系对应转换到  “北东地”的机体轴系上
    //unsigned char rxData;
    unsigned char BUF[14];
#ifdef USE_IIC_PROTOCAL
    Multiple_Read(MPU9250_IIC_ADDR, ACCEL_XOUT_H, 14, BUF);
#else
    rxData = mpu9250_read_reg(I2C_MST_STATUS);
    if (rxData & 0x01)
    {
        printf("error11111################################### \r\n");
        printf("################## I2C_MST_STATUS is :0x%2X \r\n", rxData);
        mpu9250_write_reg(USER_CTRL, 0x22);
        mpu9250_mdelay(5);
    }
    ak8963_read_magnetometer();//读取磁力计数据到MPU9250中
    BUF[0] = mpu9250_read_reg(ACCEL_XOUT_H);
    BUF[1] = mpu9250_read_reg(ACCEL_XOUT_L);
    BUF[2] = mpu9250_read_reg(ACCEL_YOUT_H);
    BUF[3] = mpu9250_read_reg(ACCEL_YOUT_L);
    BUF[4] = mpu9250_read_reg(ACCEL_ZOUT_H);
    BUF[5] = mpu9250_read_reg(ACCEL_ZOUT_L);
    BUF[6] = mpu9250_read_reg(TEMP_OUT_H);
    BUF[7] = mpu9250_read_reg(TEMP_OUT_L);
    BUF[8] = mpu9250_read_reg(GYRO_XOUT_H);
    BUF[9] = mpu9250_read_reg(GYRO_XOUT_L);
    BUF[10] = mpu9250_read_reg(GYRO_YOUT_H);
    BUF[11] = mpu9250_read_reg(GYRO_YOUT_L);
    BUF[12] = mpu9250_read_reg(GYRO_ZOUT_H);
    BUF[13] = mpu9250_read_reg(GYRO_ZOUT_L);

    BUF[14] = mpu9250_read_reg(EXT_SENS_DATA_00);
    BUF[15] = mpu9250_read_reg(EXT_SENS_DATA_01);
    BUF[16] = mpu9250_read_reg(EXT_SENS_DATA_02);
    BUF[17] = mpu9250_read_reg(EXT_SENS_DATA_03);
    BUF[18] = mpu9250_read_reg(EXT_SENS_DATA_04);
    BUF[19] = mpu9250_read_reg(EXT_SENS_DATA_05);
    BUF[20] = mpu9250_read_reg(EXT_SENS_DATA_06);
    BUF[21] = mpu9250_read_reg(EXT_SENS_DATA_07);
#endif
    //accel
    rdata_t-> accel_rx = (BUF[2] << 8) | BUF[3]; //mpu9250_accel_Y->body_accel_x
    rdata_t-> accel_ry = (BUF[0] << 8) | BUF[1]; //mpu9250_accel_X->body_accel_y
    rdata_t-> accel_rz =  -((BUF[4] << 8) | BUF[5]);//mpu9250_accel_Z->-body_accel_z
    //temp
    rdata_t->temp_r = (BUF[6] << 8) | BUF[7];
    //gyro
    rdata_t-> gyro_rx = (BUF[10] << 8) | BUF[11]; //mpu9250_gyro_Y->body_gyro_x
    rdata_t-> gyro_ry = (BUF[8] << 8) | BUF[9]; //mpu9250_gyro_X->body_gyro_y
    rdata_t-> gyro_rz =  -((BUF[12] << 8) | BUF[13]);//mpu9250_gyro_Z->-body_gyro_z

//    //*sta1 = BUF[14];
//    //mag
//    rdata_t-> mag_rx = (BUF[16] << 8) | BUF[15];
//    rdata_t-> mag_ry = (BUF[18] << 8) | BUF[17];
//    rdata_t-> mag_rz = ((BUF[20] << 8) | BUF[19]); //0.15uT/LSB
//    //The sensitivity adjustment is done by the equation below;
//    rdata_t-> mag_rx *= ((ASA_X - 128.0) * 0.5 / 128.0 + 1.0);
//    rdata_t-> mag_ry *= ((ASA_Y - 128.0) * 0.5 / 128.0 + 1.0);
//    rdata_t-> mag_rz *= ((ASA_Z - 128.0) * 0.5 / 128.0 + 1.0);
//    //*sta2 = BUF[21];

}


void mpu9250_get_data(mpu9250_rdata_t rdata_t, mpu9250_data_t data_t)
{
    //accel
    data_t-> accel_x = rdata_t-> accel_rx / 2048.0f;
    data_t-> accel_y = rdata_t-> accel_ry / 2048.0f;
    data_t-> accel_z = rdata_t-> accel_rz / 2048.0f; //unit: g
    //temp
    data_t->temp = (float)(rdata_t->temp_r - 0) / 333.87f + 21;//TEMP_degC  = ((TEMP_OUT - RoomTemp_Offset)/Temp_Sensitivity) + 21degC
    //gyro
    data_t-> gyro_x = rdata_t-> gyro_rx / 16.4f * 0.0174533f;
    data_t-> gyro_y = rdata_t-> gyro_ry / 16.4f * 0.0174533f;
    data_t-> gyro_z = rdata_t-> gyro_rz / 16.4f * 0.0174533f; //unit: deg*0.0174533->rad!!!
    //mag
//  data_t-> mag_x = rdata_t-> mag_rx * 0.15 * 0.01;
//  data_t-> mag_y = rdata_t-> mag_ry * 0.15 * 0.01;
//  data_t-> mag_z = rdata_t-> mag_rz * 0.15 * 0.01; //uT*0.01->Gass
}

void mpu9250_get_cdata(mpu9250_data_t data_t, mpu9250_cdata_t cdata_t)
{
    //数据归一化稍后放在姿态解算里面，因为归一化不涉及传感器本省的特性；校准参数应该与机体轴系对应而不是测量轴系
//    float norm;
//    float mtx, mty, mtz;
    //Calibrate Gyro
    cdata_t->gyro_cx = data_t->gyro_x - mpu9250_gyro_x_offset;
    cdata_t->gyro_cy = data_t->gyro_y - mpu9250_gyro_y_offset;
    cdata_t->gyro_cz = data_t->gyro_z - mpu9250_gyro_z_offset;

    //Calibrate Accel
    cdata_t->accel_cx = mpu9250_accel_x_scale * (data_t->accel_x) + mpu9250_accel_x_offset;
    cdata_t->accel_cy = mpu9250_accel_y_scale * (data_t->accel_y) + mpu9250_accel_y_offset;
    cdata_t->accel_cz = mpu9250_accel_z_scale * (data_t->accel_z) + mpu9250_accel_z_offset;

//    norm = sqrt((cdata_t->accel_cx * cdata_t->accel_cx +
//                 cdata_t->accel_cy * cdata_t->accel_cy +
//                 cdata_t->accel_cz * cdata_t->accel_cz));
//    cdata_t->accel_cx = cdata_t->accel_cx / norm;
//    cdata_t->accel_cy = cdata_t->accel_cy / norm;
//    cdata_t->accel_cz = cdata_t->accel_cz / norm;
//    //Calibrate Mag
//    mtx = data_t->mag_x - Mag_Bias_X;
//    mty = data_t->mag_y - Mag_Bias_Y;
//    mtz = data_t->mag_z - Mag_Bias_Z;
//    cdata_t->mag_cx = Mag_Scale [0] * mtx + Mag_Scale [1] * mty + Mag_Scale [2] * mtz;
//    cdata_t->mag_cy = Mag_Scale [1] * mtx + Mag_Scale [3] * mty + Mag_Scale [4] * mtz;
//    cdata_t->mag_cz = Mag_Scale [2] * mtx + Mag_Scale [4] * mty + Mag_Scale [5] * mtz;

//    norm = sqrt(cdata_t->mag_cx * cdata_t->mag_cx + cdata_t->mag_cy * cdata_t->mag_cy + cdata_t->mag_cz * cdata_t->mag_cz);
//    cdata_t->mag_cx = cdata_t->mag_cx / norm;
//    cdata_t->mag_cy = cdata_t->mag_cy / norm;
//    cdata_t->mag_cz = cdata_t->mag_cz / norm;


//  //加速度数据软件滤波20Hz
//  sensorData->accel_cx = ButterworthFilter_one_order_4(sensorData->accel_cx);
//  sensorData->accel_cy = ButterworthFilter_one_order_5(sensorData->accel_cy);
//  sensorData->accel_cz = ButterworthFilter_one_order_6(sensorData->accel_cz);
//  //陀螺仪数据过高通2Hz
//  sensorData->gyro_cx = ButterworthFilter_one_order_7(sensorData->gyro_cx);
//  sensorData->gyro_cy = ButterworthFilter_one_order_8(sensorData->gyro_cy);
//  sensorData->gyro_cz = ButterworthFilter_one_order_9(sensorData->gyro_cz);
}



